% -------------------------------------------------------------------------
% solve low thrust transfer with 3 body dynamics
%
% [F , J] = fsolve_cart_(p0 , auxdata)
%
% Input argumuents:
% -------------------------------------------------------------------------
% p0             [8x1]            initial co-states             [  -  ]
% auxdata        [structure]      parameters                    [  -  ]
%
% Output argumuents:
% -------------------------------------------------------------------------
% F              [8x1]            shooting constraints          [  -  ]
% J              [8x8]            jacobian matrix               [  -  ]
%
% External functions called:
% -------------------------------------------------------------------------
% ode_yphi_.m
% ode_yphi__mex.mexw32
%
% Copyright(C) 2015/06/22 by Chen Zhang,
% School of Astronautics, Beihang University
% chenzhang.buaa@gmail.com
% -------------------------------------------------------------------------

function [F , J] = fsolve_cart(p0 , auxdata)
% evaluate constraints
F = Eval_F(p0 , auxdata);
% calculate gradient of constraints
J = zeros(size(F , 1) , size(p0 , 1));
if nargout > 1
    delta = 1e-8;
    for i = 1 : size(p0 , 1)
        ddelta = zeros(size(F));
        ddelta(i) = delta;
        F_plus = Eval_F(p0 + ddelta, auxdata);
        F_minus = Eval_F(p0 - ddelta, auxdata);
        J(:, i) = (F_plus - F_minus) / (2*delta) ;
    end
end
end

function F = Eval_F(p0 , auxdata)
mu = auxdata.mu;
Tmax = auxdata.Tmax;
c = auxdata.c;
auxdata.tf = p0(end);

[tt, xx, ErrorFlag] = ode78([0 , auxdata.tf] , [auxdata.x0 ; p0(1 : 7)] , auxdata);

x1 = xx(1); x2 = xx(2); x3 = xx(3); 
x4 = xx(4); x5 = xx(5); x6 = xx(6); x7 = xx(7);
x8 = xx(8); x9 = xx(9); x10 = xx(10); 
x11 = xx(11); x12 = xx(12); x13 = xx(13); x14 = xx(14);

% calculate switching function
Sk = -(x11^2 + x12^2 + x13^2)^(1/2) * auxdata.c / x7 - x14; 
if Sk < 0
    u = 1;
else
    u = 0;
end

% calculate hamiltonian
Hamiltonian = x10*x6 - x13*((mu*x3)/((mu + x1 - 1)^2 + x2^2 + x3^2)^(3/2) -...
    (x3*(mu - 1))/((mu + x1)^2 + x2^2 + x3^2)^(3/2) + ...
    (Tmax*u*x13)/(x7*(x11^2 + x12^2 + x13^2)^(1/2))) + x4*x8 + x5*x9 - ...
    x12*(2*x4 - x2 + (mu*x2)/((mu + x1 - 1)^2 + x2^2 + x3^2)^(3/2) - ...
    (x2*(mu - 1))/((mu + x1)^2 + x2^2 + x3^2)^(3/2) + ...
    (Tmax*u*x12)/(x7*(x11^2 + x12^2 + x13^2)^(1/2))) + x11*(x1 + 2*x5 - ...
    (mu*(mu + x1 - 1))/((mu + x1 - 1)^2 + x2^2 + x3^2)^(3/2) + ...
    ((mu + x1)*(mu - 1))/((mu + x1)^2 + x2^2 + x3^2)^(3/2) - ...
    (Tmax*u*x11)/(x7*(x11^2 + x12^2 + x13^2)^(1/2))) - (Tmax*u*x14)/c + 1;

% Hamiltonian = x10*x6 - x13*((mu*x3)/((mu + x1 - 1)^2 + x2^2 + x3^2)^(3/2) - (x3*(mu - 1))/((mu + x1)^2 + x2^2 + x3^2)^(3/2) + (Tmax*u*x13)/(x7*(x11^2 + x12^2 + x13^2)^(1/2))) + x4*x8 + x5*x9 - x12*(2*x4 - x2 + (mu*x2)/((mu + x1 - 1)^2 + x2^2 + x3^2)^(3/2) - (x2*(mu - 1))/((mu + x1)^2 + x2^2 + x3^2)^(3/2) + (Tmax*u*x12)/(x7*(x11^2 + x12^2 + x13^2)^(1/2))) + x11*(x1 + 2*x5 - (mu*(mu + x1 - 1))/((mu + x1 - 1)^2 + x2^2 + x3^2)^(3/2) + ((mu + x1)*(mu - 1))/((mu + x1)^2 + x2^2 + x3^2)^(3/2) - (Tmax*u*x11)/(x7*(x11^2 + x12^2 + x13^2)^(1/2))) - (Tmax*u*x14)/c + 1;

% calculate shooting constraints
F = [xx(1 : 6)' - auxdata.xf;
    xx(14);
    Hamiltonian];

end
% -------------------------------------------------------------------------
